#pragma once

#ifndef _UTILITY_H_
#define _UTILITY_H_

#include"common_include.h"
#include"config.h"

namespace esikf_slam{

sensor_msgs::Imu imuConverter2Lidar(const sensor_msgs::Imu& imu_in, const Config::Ptr p_config);

Eigen::Matrix4d SE3toEigenM(const SE3 &pose);

void SE3toRt(const SE3 &pose,double* var);

SE3 Rt2SE3(double* var);

double compute_inlier_residual_threshold(std::vector<double> residuals, double ratio);

Eigen::Matrix<double, 3, 1> pcl_pt_to_eigend(PointType &pt);

void pcDownSample(pcl::PointCloud<PointType>::Ptr pc, double size);

sensor_msgs::PointCloud2 publishCloud(ros::Publisher& thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame);

double pointDistance(PointType p);

void anti_symmetric(Eigen::Vector3d const &_v, Eigen::Matrix3d &_m);

void tranformPoint(const PointType p_in,PointType & p_out, Eigen::Matrix4d T);

// gtsam::Pose3 SE3togtsamPose(const SE3& pose);

// SE3 gtsamPose2SE3(const gtsam::Pose3 pose);

//debug
void cout_array(double* var,int size);

Eigen::Matrix3d rightJacobiansInv(V3D fi);

void enforceSymmetry(Eigen::MatrixXd &mat);

Eigen::Matrix3d vecToMatrix(const V3D & v);

Eigen::Vector3d Quat2axis(const Eigen::Quaterniond &q);

template <typename Type>
Type wrap_pi(Type x);

}


#endif
